static uint8_t point;
static uint8_t s[128];
void serialize16(int16_t a) {SerialUSB.write( a); SerialUSB.write( (a>>8)&0xff);}
void serialize8(uint8_t a)  {SerialUSB.write( a);}

// ***********************************
// Interrupt driven UART transmitter for MIS_OSD
// ***********************************
static uint8_t tx_ptr;
static uint8_t tx_busy = 0;
uint8 writeBuff[64];
uint8 testMoto=0;
int RecWriteData(void)
{
   int count=0,i,num;
  uint32 startT;
  count=0;
  startT=millis();
  while((count<=29)&&((millis() - startT) <1000))
  {
     num=SerialUSB.available();
     //SerialUSB.print(num);
    if(num)
    {
      for(i=0;i<num;i++)
      { writeBuff[count]= SerialUSB.read( );
        count++;
      }
       
    }
  }
  
  if (count<29)return 1;
  else return 0;
}
 
void serialCom() {
  int16_t a;
  uint8_t i;
uint32 startT;
  uint16_t intPowerMeterSum, intPowerTrigger1;   
  
  if (  SerialUSB.available()) {
    switch (SerialUSB.read()) {
    #ifdef LCD_TELEMETRY
    case 'A': // button A press
      if (telemetry=='A') telemetry = 0; else { telemetry = 'A'; LCDprint(12); /* clear screen */ }
      break;    
    case 'B': // button B press
      if (telemetry=='B') telemetry = 0; else { telemetry = 'B'; LCDprint(12); /* clear screen */ }
      break;    
    case 'C': // button C press
      if (telemetry=='C') telemetry = 0; else { telemetry = 'C'; LCDprint(12); /* clear screen */ }
      break;    
    case 'D': // button D press
      if (telemetry=='D') telemetry = 0; else { telemetry = 'D'; LCDprint(12); /* clear screen */ }
      break;
    case 'a': // button A release
    case 'b': // button B release
    case 'c': // button C release
    case 'd': // button D release
      break;      
    #endif
    case 'M': // Multiwii @ arduino to GUI all data
      point=0;
      serialize8('M');
      serialize8(VERSION);  // MultiWii Firmware version
     //for(i=0;i<3;i++) serialize16 (accFirSmooth[i]);
     
      for(i=0;i<3;i++) serialize16(accSmooth[i]);
      for(i=0;i<3;i++) serialize16(gyroData[i]/8);
      for(i=0;i<3;i++) serialize16(magADC[i]/3);
      serialize16(EstAlt*10.0f);
      serialize16(heading); // compass
      for(i=0;i<4;i++) serialize16(servo[i]);
      for(i=0;i<8;i++) serialize16(motor[i]);
      for(i=0;i<8;i++) serialize16(rcData[i]);
      serialize8(nunchuk|ACC<<1|BARO<<2|MAG<<3);
      serialize8(accMode|baroMode<<1|magMode<<2);
      serialize16(cycleTime);
      for(i=0;i<2;i++) serialize16(angle[i]/10);
      serialize8(MULTITYPE);
      for(i=0;i<5;i++) {serialize8(P8[i]);serialize8(I8[i]);serialize8(DD8[i]);}
      serialize8(P8[PIDLEVEL]);
      serialize8(I8[PIDLEVEL]);
      serialize8(P8[PIDMAG]);
      serialize8(rcRate8); 
      serialize8(rcExpo8);
      serialize8(rollPitchRate);
      serialize8(yawRate);
      serialize8(dynThrPID);
      for(i=0;i<6;i++) serialize8(activate[i]);
      
      #if defined(POWERMETER)
        intPowerMeterSum = (pMeter[PMOTOR_SUM]/PLEVELDIV);
        intPowerTrigger1 = powerTrigger1 * PLEVELSCALE;
        serialize16(intPowerMeterSum);
      serialize16(intPowerTrigger1);
      #else
      serialize16(0);serialize16(0);
      #endif    
      
      serialize8(vbat);
      serialize16(BaroAlt*10.0f); // 4 variables are here for general monitoring purpose
      serialize16(0);              // debug2 for roll angle
      serialize16(0);              // debug3 for pitch angle
      serialize16(0);              // debug4
      serialize8('M');
      
      break;
    case 'O':  // arduino to OSD data - contribution from MIS
      point=0;
      serialize8('O');
      for(i=0;i<3;i++) serialize16(accSmooth[i]);
      for(i=0;i<3;i++) serialize16(gyroData[i]);
      serialize16(EstAlt*10.0f);
      serialize16(heading); // compass - 16 bytes
      for(i=0;i<2;i++) serialize16(angle[i]); //20
      for(i=0;i<6;i++) serialize16(motor[i]); //32
      for(i=0;i<6;i++) {serialize16(rcData[i]);} //44
      serialize8(nunchuk|ACC<<1|BARO<<2|MAG<<3);
      serialize8(accMode|baroMode<<1|magMode<<2);
      serialize8(vbat);     // Vbatt 47
      serialize8(VERSION);  // MultiWii Firmware version
      serialize8('O'); //49
     // UartSendData();
      break;
    case 'W': //GUI write params to eeprom @ arduino
     if(RecWriteData()!=0){SerialUSB.print("err");return;}
     
     {  
        int dataIndex=0;
      for(i=0;i<5;i++) {P8[i]=writeBuff[dataIndex++]; I8[i]= writeBuff[dataIndex++]; DD8[i]=writeBuff[dataIndex++];} //9
      P8[PIDLEVEL] = writeBuff[dataIndex++]; I8[PIDLEVEL] = writeBuff[dataIndex++]; //11
      P8[PIDMAG] = writeBuff[dataIndex++];
      rcRate8 = writeBuff[dataIndex++]; rcExpo8 = writeBuff[dataIndex++];
      rollPitchRate = writeBuff[dataIndex++]; yawRate =writeBuff[dataIndex++]; //16
      dynThrPID =writeBuff[dataIndex++];
      for(i=0;i<6;i++) activate[i] =writeBuff[dataIndex++]; //22
     #if defined(POWERMETER)
      powerTrigger1 = (writeBuff[dataIndex++] + 256* writeBuff[dataIndex++] ) / PLEVELSCALE; // we rely on writeParams() to compute corresponding pAlarm value
     #else
      writeBuff[dataIndex++];writeBuff[dataIndex++]; // so we unload the two bytes
     #endif
      writeParams();
     }
    //  SerialUSB.print("OK");
      break;
    case 'S': //GUI to arduino ACC calibration request
      calibratingA=400;
      break;
    case 'E': //GUI to arduino MAG calibration request
      calibratingM=1;
      break;
      case 'R':
      SetParamDefault();
      break;
      case 'G':
      uint8 tmpASK;	 
        tmpASK=0;
       Wire.readMultiByte(0x1E,0x0A,&tmpASK,1);
	   SerialUSB.print("hmc5833:");SerialUSB.print((char)tmpASK);
	    tmpASK=0;
	   Wire.readMultiByte(0x68,0x75,&tmpASK,1);
	   SerialUSB.print("\r\nmpu6050:");SerialUSB.print(tmpASK,16);
	    tmpASK=0;
	   Wire.readMultiByte(0x1c,0x0d,&tmpASK,1);
      //tmpASK=i2c_readReg(0X1C,0X0D);
	  SerialUSB.print("\r\nMMA:");SerialUSB.print(tmpASK,16);
	  //tmpASK=i2c_readReg(0X77,0Xa0);
	   tmpASK=0;
	  	Wire.readMultiByte(0x77,0xa2,&tmpASK,1);
	   SerialUSB.print("\r\nMs5611:");SerialUSB.print(pressure,16);
         //uint8_t rawData[3];
        // Wire.readMultiByte(MS561101BA_ADDRESS,0,rawData,3);
        //  SerialUSB.print("\r\nRAW:");SerialUSB.print(rawData[0],16);SerialUSB.print(rawData[1],16);SerialUSB.print(rawData[2],16);
	//i2c_MS561101BA_UP_Read () ;
   SerialUSB.print("\r\ntest:");
   SerialUSB.print(ms561101ba_ctx.up.val,16);
  
         SerialUSB.print("\r\nBaroAlt:");SerialUSB.print( BaroAlt,16);
	  
	   
      SerialUSB.print("\r\naccZero0:");SerialUSB.print(accZero[0]);
	  SerialUSB.print("\t accZero1:");SerialUSB.print(accZero[1]);
	  SerialUSB.print("\t accZero2:"); SerialUSB.print(accZero[2]);
	  SerialUSB.print("\r\n");

	   SerialUSB.print("gyroZero0:");SerialUSB.print(gyroZero[0]);
	  SerialUSB.print("\t gyroZero1:");SerialUSB.print(gyroZero[1]);
	  SerialUSB.print("\t gyroZero2:"); SerialUSB.print(gyroZero[2]);
	  SerialUSB.print("\r\n");
      //serialize8(i2c_readReg(ITG3200_ADDRESS,0x00));
      //serialize8(i2c_readReg(ITG3200_ADDRESS,0x1d));serialize8(i2c_readReg(ITG3200_ADDRESS,0x1e));
      //serialize8(i2c_readReg(ITG3200_ADDRESS,0x1f));serialize8(i2c_readReg(ITG3200_ADDRESS,0x20));
     // serialize8(i2c_readReg(ITG3200_ADDRESS,0x21));serialize8(i2c_readReg(ITG3200_ADDRESS,0x22));
      
      break;
      case 'L':
        
       serialize8(SI4432Read(0x01));
      
       serialize8(SI4432Read(0x53));
         
       serialize8(SI4432Read(0x04));
       serialize8(SI4432Read(0x55));
         serialize8(SI4432Read(0x54));
       break;
        case 'i' :
        serialize8(SI4432Read(0x53));//45
         serialize8(SI4432Read(0x3a));
           serialize8(SI4432Read(0x3b));
             serialize8(SI4432Read(0x3c));
               serialize8(SI4432Read(0x3d));
        serialize8(SI4432Read(0x1D));//40
        serialize8(SI4432Read(0x6e));//0a
        serialize8(SI4432Read(0x6f));//aa
        serialize8(SI4432Read(0x71));//23
        serialize8(SI4432Read(0x72));//43
        serialize8(SI4432Read(0x76));//bb
         serialize8(SI4432Read(0x77));//80
     break;
#if (MAG)
    case 'j':
    {
      uint8 tmpbuff[8],i;
       
       
        Device_Mag_getADC() ;
         for(i=0;i<3;i++)serialize16(magADC[i]);
         
          i2c_getSixRawADC(LSM303DLHC_M_ADDR,0x80|0x03);   
        for(i=0;i<6;i++)serialize8(rawADC[i]);
        
        ACC_getADC(); 
        for(i=0;i<3;i++)serialize16(gyroZero[i]);
        for(i=0;i<3;i++)serialize16(accZero[i]);
        i2c_getSixRawADC(LSM303DLHC_A_ADDR,0x80|0x28);      
        for(i=0;i<6;i++)serialize8(rawADC[i]);
        
        
        
         i2c_getSixRawADC(L3G4200D_ADDR,0x80|0x28);      
        for(i=0;i<6;i++)serialize8(rawADC[i]);
    }
   break;
 #endif
  case '1':
  for(i=0;i<4;i++)
    serialize16(motor[i]);
   break;
  case '2':
   break;
   case '3':
   break;
   case '4':
   break;
    }
   
  }
}
